#include "cg_remote_control/udp_backend.hpp"
#include <arpa/inet.h>
#include <cstring>
#include <fcntl.h>
#include <unistd.h>
#include "ros/ros.h"

namespace CG
{
    UDPBackEnd::UDPBackEnd(NetEndPoint local)
    {
        socketfd = socket(AF_INET, SOCK_DGRAM, 0);
        sockaddr_in addr = local.Addr();
        if (!SetSocketUnblock(socketfd))
        {
            throw "Set Socket Unblock Fail";
        }
        if (!SetSocketAbleToBoardCast(socketfd))
        {
            throw "Set Socket Boardcast Fail";
        }
        int ret = bind(socketfd, (sockaddr *)&addr, (socklen_t)sizeof(addr));
        if (ret < 0)
            throw "Socket bind address failed";
    }

    UDPBackEnd::~UDPBackEnd()
    {
        if (socketfd > 0)
            close(socketfd);
    }

    bool UDPBackEnd::Send(const unsigned char *const data, size_t length, const NetEndPoint &target)
    {

        auto addr = target.Addr();
        int ret = sendto(socketfd, data, length, 0, (sockaddr *)&addr, sizeof(sockaddr_in));
        return ret == length;
    }

    bool UDPBackEnd::Receive(unsigned char *data, size_t bufferSize, ssize_t &length, NetEndPoint &source)
    {
        sockaddr_in senderAddr;
        bzero(&senderAddr, sizeof(sockaddr_in));
        unsigned int len = sizeof(sockaddr_in);

        length = recvfrom(socketfd, data, bufferSize, 0, (sockaddr *)&senderAddr, (socklen_t *)&len);
        if (length == 0 || (length == -1 && errno == EAGAIN))
        {
            return false;
        }
        if (length == -1 && errno != EAGAIN)
        {
            ROS_ERROR("recieve data fail!");
            return false;
        }
        source = NetEndPoint(senderAddr);
        return true;
    }

    bool UDPBackEnd::SetSocketUnblock(int socketfd)
    {
        int flag = fcntl(socketfd, F_GETFL, 0);
        if (flag < 0)
        {
            return false;
        }
        if (fcntl(socketfd, F_SETFL, flag | O_NONBLOCK) < 0)
        {
            return false;
        }
        return true;
    }

    bool UDPBackEnd::SetSocketAbleToBoardCast(int socketfd)
    {
        int so_broadcast = 1;
        if (setsockopt(socketfd, SOL_SOCKET, SO_BROADCAST, &so_broadcast, sizeof(so_broadcast)) < 0)
        {
            close(socketfd);
            return false;
        }
        return true;
    }
}